/*
 * legotasks.c
 *
 *  Created on: May 7, 2010
 *      Author: Armen Nazarian
 *
 *
 * In this c file we have tasks that control the components on the lego conveyor belt.
 * This file is only for tasks that are run by uOS.
 *
 */

#include <includes.h>
#include <../lego_interface/lego_interface.h>
#include <toolbox/toolbox.h>
#include "legotasks.h"

static INT8U On_flag_first_belt;
static INT8U On_flag_second_belt;
static OS_EVENT *first_belt_sem;
static OS_EVENT *sort_sem;
static INT8U next_brick_color;
static INT8U buffer_value_sempahore_value;

static INT8U brick_count;

/*
 * The init function set the variables that are used by multply tasks and are set in the
 * app.c file. This function must be run before using the tasks inside here.
 *
 * Parameters:
 * 		OS_EVENT *sem_first_belt :		the semaphore to control the buffer
 * 		INT8U flag_first_belt	 :		1 if the first belt should run from beginning and 0 otherwise
 *		INT8U flag_second_belt	 :		same is the above but for the second belt.
 *		INT8U buffer_value		 :		One of the tasks uses the initial value of the semaphore created in app.c to for security.
 *
 *	Return:
 *		void
 *
 */
void init_tasks(OS_EVENT *sem_first_belt,OS_EVENT *sem_sort, INT8U flag_first_belt,
		INT8U flag_second_belt, INT8U buffer_value) {
	On_flag_first_belt = flag_first_belt;
	On_flag_second_belt = flag_first_belt;
	first_belt_sem = sem_first_belt;
	sort_sem = sem_sort;
	buffer_value_sempahore_value = buffer_value;

	brick_count = 0;

}

/*
 * This tasks handles whenever a brick is read from the light sensor
 *
 *
 */

void sim_brick(void *p_arg) {
	(void) p_arg;
	INT8U *err;
	OSTimeDly(OS_TICKS_PER_SEC / 5); // give sensor time to be ready. (it might flickr in the beginning)

	while (1) {
		INT8U number_read = (light_sensor(1) >> 2);
		if (number_read < 200) {

			On_flag_first_belt = emergency_stop_first_belt(); //emergency stop and save the flag result

				OSSemPend(first_belt_sem, 0, err); // decrement/pend the sempahore
				brick_count++; // count brick up by one.


			OSTimeDly(OS_TICKS_PER_SEC / 30); // delaying so the sensors doesn't read twice or more

			On_flag_first_belt = emergency_over_first_belt(); //run the belt again, HARD. And set the flag so the belt continues to run.
		} else {
		}
		OSTimeDly(OS_TICKS_PER_SEC / 5);
	}

}

/*
 *
 * Runs the first belt if the flag is set.
 */

void first_belt_run(void *p_arg) {
	(void) p_arg;
	while (1) {
		if (On_flag_first_belt) {
			motor_speed(1, 60);
		} else {
			motor_speed(1, 0);
		}
		OSTimeDly(OS_TICKS_PER_SEC / 5);

	}

}

/*
 * This task handles if the light_sensor reads a brick right before the second belt.
 * If it does it will rotate the brick unto the second belt.
 * It has a security against posting more than the initali value.
 */
void rotate_unto_next(void *p_arg) {
	(void) p_arg;
	OSTimeDly(OS_TICKS_PER_SEC); //give time for the light sensor to flickr
	INT8U number_read;
	while (1) {
		number_read = (light_sensor(0) >> 2);

		if (number_read < 200) {
			/* checks the color and sets the variable brick_color for the sorting part*/

			if (176 > number_read) { // the brick is yellow
				next_brick_color = 1;
			} else { // the brick is not yellow
				next_brick_color = 0;
			}

			OSTimeDly(OS_TICKS_PER_SEC * 5);
			motor_speed(0, 3); // rotate the switch
			OSTimeDly(OS_TICKS_PER_SEC / 3); //delaying so the switch will rotate enough
			brake_motor(0); // stop rotating

			/* the following is a security againts 2 bricks going unto the second belt*/
			/* the follwing 3 lines rotates the switch back a little for security*/

			motor_speed(0, -3);
			OSTimeDly(OS_TICKS_PER_SEC / 4);
			brake_motor(0);

			/*its security against overloading the buffer*/
			/* we make sure that the brick_count is where its supposed to be*/

			if (brick_count > 0) {
				OSSemPost(first_belt_sem);
				brick_count--;

			}

			OSTimeDly(OS_TICKS_PER_SEC / 5);

			if (next_brick_color) {

				OSSemPost(sort_sem);// runs the brick sorter if the brick is yellow
			}

		}
		OSTimeDly(OS_TICKS_PER_SEC / 20);
	}
}

/*The following task runs the second belt if its flag is set*/

void second_belt(void *p_arg) {
	(void) p_arg;
	while (1) {
		if (On_flag_second_belt) {
			motor_speed(2, 5);
		} else {
			motor_speed(2, 0);
		}
		OSTimeDly(OS_TICKS_PER_SEC / 5);

	}

}

/*The handles the sorter, its stops it if it touches its touch sensor,
 * this means that it has rotated 1 round
 */

void touch_sensor_reader(void *p_arg) {
	(void) p_arg;

	while (1) {
		INT8U number_read = (light_sensor(2) >> 2);

		if (number_read < 197) {
			brick_sorter_halt();

		}

		OSTimeDly(OS_TICKS_PER_SEC / 80);
	}
}

void run_sorter(void *p_arg){
	(void) p_arg;
	INT8U *perr;
	while (1){

		OSSemPend(sort_sem, 0, perr);
		motor_speed(3,-16);
		OSTimeDly(OS_TICKS_PER_SEC / 5);
	}
}

